package flexo.math;

import java.nio.FloatBuffer;

/**
 * Based on http://www.whisqu.se/per/docs/math22.htm
 */
public class Matrix implements Comparable<Matrix>
{
	private static final int NUM_TEMP_MATRICES = 4;
	private static Matrix[] tempMatrices;
	private static int tempIterator;
	
	static
	{
		tempMatrices = new Matrix[NUM_TEMP_MATRICES];
		for (int i = 0; i < NUM_TEMP_MATRICES; ++i)
		{
			tempMatrices[i] = new Matrix();
		}
		tempIterator = 0;
	}
	
	public static Matrix getTempMatrix()
	{
		if (tempIterator >= NUM_TEMP_MATRICES)
		{
			tempIterator = 0;
		}
		
		return tempMatrices[tempIterator++];
	}
	
	/*
	 * OpenGL indices for a transformation 4x4 matrix.
	 * @author Miguel
	 */
	protected final static int[][] index = {{0,4,8,12},{1,5,9,13},{2,6,10,14},{3,7,11,15}};
	
	protected static final byte DATA_COUNT = 16;
	protected FloatBuffer data;
	protected boolean isReset = false;
	
	public Matrix()
	{
		data = FloatBuffer.allocate(DATA_COUNT);
		reset();
	}
	
	public Matrix(Vector x, Vector y, Vector z, Vector pos)
	{
		this(pos);
		
		data.put(index[0][0], x.x);
		data.put(index[1][0], x.y);
		data.put(index[2][0], x.z);
		
		data.put(index[0][1], y.x);
		data.put(index[1][1], y.y);
		data.put(index[2][1], y.z);
		
		data.put(index[0][2], z.x);
		data.put(index[1][2], z.y);
		data.put(index[2][2], z.z);
	}
	
	public Matrix(Vector position)
	{
		this();
		setPosition(position);
	}
		
	public Matrix(Quaternion rotation)
	{
		this();
		setRotation(rotation);
	}
	
	public Matrix(Matrix m)
	{
		this();
		load(m);
	}	
	
	public FloatBuffer getDataBuffer()
	{
		data.rewind();
		return data;
	}
	
	/**
	 * Transforms the vector into another using the matrix.
	 * The vector's values are overwritten.
	 */
	public void transform(Vector v)
	{
		Vector original = Vector.getTempVector();
		original.set(v);
		transform(this,original,v);
	}
	
	public void load(Matrix m)
	{
		if (m != this)
		{
			m.data.rewind();
			data.rewind();
			data.put(m.data);
			data.rewind();
		}
	}
	
	public void inverseTransform(Vector v)
	{
		Vector original = Vector.getTempVector();
		original.set(v);
		inverseTransform(this, original, v);
	}
	
	/**
	 * Resets the values to build an identity matrix.
	 */
	public void reset()
	{
		if (!isReset)
		{
			for (int i = 0; i < DATA_COUNT; ++i)
			{
				if (i == 0 || i == 5 || i == 10 || i == 15)
				{
					data.put(i,1.0f);
				}
				else
				{
					data.put(i,0.0f);
				}
			}
			isReset = true;
		}
	}
	
	public void transpose()
	{
		float[] temp = new float[6];
		
		temp[0] = data.get(index[0][1]);
		temp[1] = data.get(index[0][2]);
		temp[2] = data.get(index[0][3]);
		temp[3] = data.get(index[1][2]);
		temp[4] = data.get(index[1][3]);
		temp[5] = data.get(index[2][3]);
		
		data.put(index[0][1],data.get(index[1][0]));
		data.put(index[0][2],data.get(index[2][0]));
		data.put(index[0][3],data.get(index[3][0]));
		data.put(index[1][2],data.get(index[2][1]));
		data.put(index[1][3],data.get(index[3][1]));
		data.put(index[2][3],data.get(index[3][2]));
		
		data.put(index[1][0],temp[0]);
		data.put(index[2][0],temp[1]);
		data.put(index[3][0],temp[2]);
		data.put(index[2][1],temp[3]);
		data.put(index[3][1],temp[4]);
		data.put(index[3][2],temp[5]);
		
		isReset = false;
	}
	
	public void setScale(Vector v)
	{
		reset();
		data.put(index[0][0], v.x);
		data.put(index[1][1], v.y);
		data.put(index[2][2], v.z);
		isReset = false;
	}
	
	public void setPosition(Vector v)
	{
		reset();
		data.put(index[0][3], v.x);
		data.put(index[1][3], v.y);
		data.put(index[2][3], v.z);
		isReset = false;
	}
	
	/**
	 * From gpwiki.org
	 * @param q
	 */
	public void setRotation(Quaternion q)
	{
		reset();
		float x2 = q.x * q.x;
		float y2 = q.y * q.y;
		float z2 = q.z * q.z;
		float xy = q.x * q.y;
		float xz = q.x * q.z;
		float yz = q.y * q.z;
		float wx = q.w * q.x;
		float wy = q.w * q.y;
		float wz = q.w * q.z;
		
		data.put(index[0][0], 1.0f - 2.0f * (y2 + z2) );
		data.put(index[0][1], 2.0f * (xy - wz) );
		data.put(index[0][2], 2.0f * (xz + wy) );
		
		data.put(index[1][0], 2.0f * (xy + wz) );
		data.put(index[1][1], 1.0f - 2.0f * (x2 + z2) );
		data.put(index[1][2], 2.0f * (yz - wx) );
		
		data.put(index[2][0], 2.0f * (xz - wy) );
		data.put(index[2][1], 2.0f * (yz + wx));
		data.put(index[2][2], 1.0f - 2.0f * (x2 + y2));	
		isReset = false;
	}
	
	/**
	 * Transforms the matrix 'm' into another using the matrix.
	 * The m's values are overwritten.
	 */
	public void transform(Matrix m)
	{
		Matrix op2 = Matrix.getTempMatrix();
		op2.set(m);
		mul(this,op2,m);
	}
	
	public void set(Matrix m)
	{
		this.data.clear();
		this.data.put(m.data);
	}
	
	public boolean isIdentity()
	{
		for (int i = 0; i < DATA_COUNT; ++i)
		{
			if (i == index[0][0] || i == index[1][1] || i == index[2][2] || i == index[3][3])
			{
				if (data.get(i) != 1) return false;
			}
			else
			{
				if (data.get(i) != 0) return false;
			}
		}
		
		return true;
	}
	
	public static void mul(Matrix op1, Matrix op2, Matrix result)
	{
		if (op1.isIdentity())
		{
			result.load(op2);
			return;
		}
		if (op2.isIdentity())
		{
			result.load(op1);
			return;
		}
		
		result.data.put(index[0][0], op1.data.get(index[0][0])*op2.data.get(index[0][0]) + op1.data.get(index[0][1])*op2.data.get(index[1][0]) + op1.data.get(index[0][2])*op2.data.get(index[2][0]) + op1.data.get(index[0][3])*op2.data.get(index[3][0]));
		result.data.put(index[0][1], op1.data.get(index[0][0])*op2.data.get(index[0][1]) + op1.data.get(index[0][1])*op2.data.get(index[1][1]) + op1.data.get(index[0][2])*op2.data.get(index[2][1]) + op1.data.get(index[0][3])*op2.data.get(index[3][1]));
		result.data.put(index[0][2], op1.data.get(index[0][0])*op2.data.get(index[0][2]) + op1.data.get(index[0][1])*op2.data.get(index[1][2]) + op1.data.get(index[0][2])*op2.data.get(index[2][2]) + op1.data.get(index[0][3])*op2.data.get(index[3][2]));
		result.data.put(index[0][3], op1.data.get(index[0][0])*op2.data.get(index[0][3]) + op1.data.get(index[0][1])*op2.data.get(index[1][3]) + op1.data.get(index[0][2])*op2.data.get(index[2][3]) + op1.data.get(index[0][3])*op2.data.get(index[3][3]));
		result.data.put(index[1][0], op1.data.get(index[1][0])*op2.data.get(index[0][0]) + op1.data.get(index[1][1])*op2.data.get(index[1][0]) + op1.data.get(index[1][2])*op2.data.get(index[2][0]) + op1.data.get(index[1][3])*op2.data.get(index[3][0]));
		result.data.put(index[1][1], op1.data.get(index[1][0])*op2.data.get(index[0][1]) + op1.data.get(index[1][1])*op2.data.get(index[1][1]) + op1.data.get(index[1][2])*op2.data.get(index[2][1]) + op1.data.get(index[1][3])*op2.data.get(index[3][1]));
		result.data.put(index[1][2], op1.data.get(index[1][0])*op2.data.get(index[0][2]) + op1.data.get(index[1][1])*op2.data.get(index[1][2]) + op1.data.get(index[1][2])*op2.data.get(index[2][2]) + op1.data.get(index[1][3])*op2.data.get(index[3][2]));
		result.data.put(index[1][3], op1.data.get(index[1][0])*op2.data.get(index[0][3]) + op1.data.get(index[1][1])*op2.data.get(index[1][3]) + op1.data.get(index[1][2])*op2.data.get(index[2][3]) + op1.data.get(index[1][3])*op2.data.get(index[3][3]));
		result.data.put(index[2][0], op1.data.get(index[2][0])*op2.data.get(index[0][0]) + op1.data.get(index[2][1])*op2.data.get(index[1][0]) + op1.data.get(index[2][2])*op2.data.get(index[2][0]) + op1.data.get(index[2][3])*op2.data.get(index[3][0]));
		result.data.put(index[2][1], op1.data.get(index[2][0])*op2.data.get(index[0][1]) + op1.data.get(index[2][1])*op2.data.get(index[1][1]) + op1.data.get(index[2][2])*op2.data.get(index[2][1]) + op1.data.get(index[2][3])*op2.data.get(index[3][1]));
		result.data.put(index[2][2], op1.data.get(index[2][0])*op2.data.get(index[0][2]) + op1.data.get(index[2][1])*op2.data.get(index[1][2]) + op1.data.get(index[2][2])*op2.data.get(index[2][2]) + op1.data.get(index[2][3])*op2.data.get(index[3][2]));
		result.data.put(index[2][3], op1.data.get(index[2][0])*op2.data.get(index[0][3]) + op1.data.get(index[2][1])*op2.data.get(index[1][3]) + op1.data.get(index[2][2])*op2.data.get(index[2][3]) + op1.data.get(index[2][3])*op2.data.get(index[3][3]));
		result.data.put(index[3][0], op1.data.get(index[3][0])*op2.data.get(index[0][0]) + op1.data.get(index[3][1])*op2.data.get(index[1][0]) + op1.data.get(index[3][2])*op2.data.get(index[2][0]) + op1.data.get(index[3][3])*op2.data.get(index[3][0]));
		result.data.put(index[3][1], op1.data.get(index[3][0])*op2.data.get(index[0][1]) + op1.data.get(index[3][1])*op2.data.get(index[1][1]) + op1.data.get(index[3][2])*op2.data.get(index[2][1]) + op1.data.get(index[3][3])*op2.data.get(index[3][1]));
		result.data.put(index[3][2], op1.data.get(index[3][0])*op2.data.get(index[0][2]) + op1.data.get(index[3][1])*op2.data.get(index[1][2]) + op1.data.get(index[3][2])*op2.data.get(index[2][2]) + op1.data.get(index[3][3])*op2.data.get(index[3][2]));
		result.data.put(index[3][3], op1.data.get(index[3][0])*op2.data.get(index[0][3]) + op1.data.get(index[3][1])*op2.data.get(index[1][3]) + op1.data.get(index[3][2])*op2.data.get(index[2][3]) + op1.data.get(index[3][3])*op2.data.get(index[3][3]));
	}
	
	public static void transform(Matrix transformation, Vector original, Vector result)
	{
		result.x = original.x*transformation.data.get(index[0][0]) + original.y*transformation.data.get(index[0][1]) + original.z*transformation.data.get(index[0][2]) + transformation.data.get(index[0][3]);
		result.y = original.x*transformation.data.get(index[1][0]) + original.y*transformation.data.get(index[1][1]) + original.z*transformation.data.get(index[1][2]) + transformation.data.get(index[1][3]);
		result.z = original.x*transformation.data.get(index[2][0]) + original.y*transformation.data.get(index[2][1]) + original.z*transformation.data.get(index[2][2]) + transformation.data.get(index[2][3]);
	}
	
	public static void inverseTransform(Matrix transformation, Vector original, Vector result)
	{
		// Calculating determinant of rotation 3x3 matrix
		float determinant = 0;
		determinant = transformation.data.get(index[0][0]) * transformation.data.get(index[1][1]) * transformation.data.get(index[2][2]);
		determinant += transformation.data.get(index[0][1]) * transformation.data.get(index[1][2]) * transformation.data.get(index[2][0]);
		determinant += transformation.data.get(index[0][2]) * transformation.data.get(index[2][1]) * transformation.data.get(index[1][0]);
		
		determinant -= transformation.data.get(index[2][0]) * transformation.data.get(index[1][1]) * transformation.data.get(index[0][2]);
		determinant -= transformation.data.get(index[2][1]) * transformation.data.get(index[1][2]) * transformation.data.get(index[0][0]);
		determinant -= transformation.data.get(index[2][2]) * transformation.data.get(index[0][1]) * transformation.data.get(index[1][0]);
		
		if (determinant != 0)
		{
			// Calculating inverse of rotation 3x3 matrix
			float a00, a01,a02,a10,a11,a12,a20,a21,a22 = 0;
			a00 = transformation.data.get(index[1][1]) * transformation.data.get(index[2][2]);
			a00 -= transformation.data.get(index[2][1]) * transformation.data.get(index[1][2]);
			a00 /= determinant;
			
			a01 = transformation.data.get(index[0][2]) * transformation.data.get(index[2][1]);
			a01 -= transformation.data.get(index[2][2]) * transformation.data.get(index[2][1]);
			a01 /= determinant;
			
			a02 = transformation.data.get(index[0][1]) * transformation.data.get(index[1][2]);
			a02 -= transformation.data.get(index[1][1]) * transformation.data.get(index[0][2]);
			a02 /= determinant;
			
			a10 = transformation.data.get(index[1][2]) * transformation.data.get(index[2][0]);
			a10 -= transformation.data.get(index[2][2]) * transformation.data.get(index[1][0]);
			a10 /= determinant;
			
			a11 = transformation.data.get(index[0][0]) * transformation.data.get(index[2][2]);
			a11 -= transformation.data.get(index[2][0]) * transformation.data.get(index[0][2]);
			a11 /= determinant;
			
			a12 = transformation.data.get(index[0][2]) * transformation.data.get(index[1][0]);
			a12 -= transformation.data.get(index[1][2]) * transformation.data.get(index[0][0]);
			a12 /= determinant;
			
			a20 = transformation.data.get(index[1][0]) * transformation.data.get(index[2][1]);
			a20 -= transformation.data.get(index[2][0]) * transformation.data.get(index[1][1]);
			a20 /= determinant;
			
			a21 = transformation.data.get(index[0][1]) * transformation.data.get(index[2][0]);
			a21 -= transformation.data.get(index[2][1]) * transformation.data.get(index[0][0]);
			a21 /= determinant;
			
			a22 = transformation.data.get(index[0][0]) * transformation.data.get(index[1][1]);
			a22 -= transformation.data.get(index[1][0]) * transformation.data.get(index[0][1]);
			a22 /= determinant;
			
			result.x = original.x - transformation.data.get(index[0][3]);
			result.y = original.y - transformation.data.get(index[1][3]);
			result.z = original.z - transformation.data.get(index[2][3]);
			
			result.x = original.x*a00 + original.y*a01 + original.z*a02;
			result.y = original.x*a10 + original.y*a11 + original.z*a12;
			result.z = original.x*a20 + original.y*a21 + original.z*a22;
		}		
	}
	
	public String toString()
	{
		StringBuffer sb = new StringBuffer();
		sb.append("[" + data.get(index[0][0]) + "]\t[" + data.get(index[0][1]) + "]\t[" + data.get(index[0][2]) + "]\t[" + data.get(index[0][3]) + "]\n");
		sb.append("[" + data.get(index[1][0]) + "]\t[" + data.get(index[1][1]) + "]\t[" + data.get(index[1][2]) + "]\t[" + data.get(index[1][3]) + "]\n");
		sb.append("[" + data.get(index[2][0]) + "]\t[" + data.get(index[2][1]) + "]\t[" + data.get(index[2][2]) + "]\t[" + data.get(index[2][3]) + "]\n");
		sb.append("[" + data.get(index[3][0]) + "]\t[" + data.get(index[3][1]) + "]\t[" + data.get(index[3][2]) + "]\t[" + data.get(index[3][3]) + "]\n");
		return sb.toString();
	}

	@Override
	public int compareTo(Matrix arg0)
	{
		if (this.data.equals(arg0.data))
		{
			return 0;
		}
		else
		{
			return -1;
		}
	}
}
